#include <Adafruit_PWMServoDriver.h>
#include "Adafruit_16ch_Servo.h"

void Adafruit_16ch_Servo::initialize()
{
	int i;

	for (i = 0; i = 15; i++)
	{
		center_pulse_width[i] = ADA16SV_DEFAULT_CENTER_PULSE_WIDTH;
		min_pulse_width[i] = ADA16SV_DEFAULT_MIN_PULSE_WIDTH;
		max_pulse_width[i] = ADA16SV_DEFAULT_MAX_PULSE_WIDTH;
	}
}

void Adafruit_16ch_Servo::setCenterPulseWidth(int ch, double width)
{
	if (ch < 0 || ch >15)	return;

	center_pulse_width[ch] = width;
}

void Adafruit_16ch_Servo::setMinPulseWidth(int ch, double width)
{
	if (ch < 0 || ch >15)	return;

	min_pulse_width[ch] = width;
}

void Adafruit_16ch_Servo::setMaxPulseWidth(int ch, double width)
{
	if (ch < 0 || ch >15)	return;

	max_pulse_width[ch] = width;
}

void Adafruit_16ch_Servo::setServoPosition(int ch, double position)
{
	if (ch < 0 || 15 < ch)	return;

	if (position < -100.0)	position = -100.0;
	if (position > 100.0)		position = 100.0;

	if (position == 0.0)
	{
		setServoPulse(ch, center_pulse_width[ch]);
	}
	else if (position < 0.0)
	{
		setServoPulse(ch, 
			(center_pulse_width[ch] - max_pulse_width[ch]) * position / 100.0
			+ center_pulse_width[ch]);
	}
	else
	{
		setServoPulse(ch,
			(min_pulse_width[ch] - center_pulse_width[ch]) * position / 100.0
			+ center_pulse_width[ch]);
	}
}